#include "dev_mpu9250.h"

#define MPU9250_AD0_ADDR         0
#define MPU9250_I2C_IMU_ADDR     (0x68 | MPU9250_AD0_ADDR)  /* 7bit 地址, 如果是8bit从机地址, 则在rt-thread驱动中, 会自动 << 1 */
#define MPU9250_I2C_MAG_ADDR     (0x0C)                     /* 从机地址 (在rt-thread驱动中, 会自动 << 1) */
#define MPU9250_I2C_BUS_NAME      "i2c2"      /* 传感器连接的I2C总线设备名称 */

/* 10轴 设备管理结构体 */
static dev_rtdrv_mpu9250_t s_dev_rtdrv_mpu9250;

/* 找到i2c控制总线 */
static RET_STATUS s_dev_mpu9250_find_i2c_bus(dev_rtdrv_mpu9250_t rtdrv_mpu9250)
{
    const char *i2c_bus_name = MPU9250_I2C_BUS_NAME;

    rtdrv_mpu9250->i2c_bus = (struct rt_i2c_bus_device *) rt_device_find(MPU9250_I2C_BUS_NAME);

    if (rtdrv_mpu9250->i2c_bus == RT_NULL)
    {
        rt_kprintf("can't find %s device!\n", i2c_bus_name);

        return RET_ERROR;
    }

    /* 设置从机地址 */
    s_dev_rtdrv_mpu9250->imu_addr = MPU9250_I2C_IMU_ADDR;
    s_dev_rtdrv_mpu9250->mag_addr = MPU9250_I2C_MAG_ADDR;

    return RET_OK;
}

/* 写寄存器 */
static RET_STATUS s_dev_mpu9250_write_regs(uint8_t target_dev, uint8_t *regs, uint8_t *data_buf, uint16_t len)
{
    struct rt_i2c_msg msgs;
    uint8_t           merge_buf[0x0F] = {0};    /* 最大申请 0x0F 字节的数组空间 */
    uint8_t           slave_addr = 0;

    /* 设置从机地址 */
    if (target_dev == DEV_MPU9250_TARGET_IMU)
    {
        slave_addr = s_dev_rtdrv_mpu9250->imu_addr;
    }
    else if (target_dev == DEV_MPU9250_TARGET_MAG)
    {
        slave_addr = s_dev_rtdrv_mpu9250->mag_addr;
    }

    /* 合并数据 */
    memcpy(&merge_buf[0], regs, DEV_MPU9250_REG_NOB);          /* 目标寄存器地址部分字节 */
    memcpy(&merge_buf[0 + DEV_MPU9250_REG_NOB], data_buf, len);    /* 往目标寄存器地址写入数据部分字节 */

    /* 构建数据包, 发送数据 */
    msgs.addr  = slave_addr;
    msgs.flags = RT_I2C_WR;
    msgs.buf   = merge_buf; /* 发送合并后的数组数据 */
    msgs.len   = DEV_MPU9250_REG_NOB + len;

    if (rt_i2c_transfer(s_dev_rtdrv_mpu9250->i2c_bus, &msgs, 1) != 1)
    {
        return RET_ERROR;   /* 传输失败 */
    }

    return RET_OK;
}

/* 读寄存器 */
static RET_STATUS s_dev_mpu9250_read_regs(uint8_t target_dev, uint8_t *regs, uint8_t *data_buf, uint16_t len)
{
    struct rt_i2c_msg msgs[2];
    uint8_t           slave_addr = 0;

    /* 设置从机地址 */
    if (target_dev == DEV_MPU9250_TARGET_IMU)
    {
        slave_addr = s_dev_rtdrv_mpu9250->imu_addr;
    }
    else if (target_dev == DEV_MPU9250_TARGET_MAG)
    {
        slave_addr = s_dev_rtdrv_mpu9250->mag_addr;
    }
    /* 构建数据包, 发送数据 */
    msgs[0].addr  = slave_addr;
    msgs[0].flags = RT_I2C_WR;
    msgs[0].buf   = regs;
    msgs[0].len   = DEV_MPU9250_REG_NOB;

    msgs[1].addr  = slave_addr;
    msgs[1].flags = RT_I2C_RD;
    msgs[1].buf   = data_buf;
    msgs[1].len   = len;

    if (rt_i2c_transfer(s_dev_rtdrv_mpu9250->i2c_bus, msgs, 2) != 2)
    {
        return RET_ERROR;   /* 传输失败 */
    }

    return RET_OK;
}

/* 触摸屏寄存器配置 */
static RET_STATUS s_dev_mpu9250_reg_config(void)
{
    uint8_t  reg[1] = {0};
    uint8_t  buf[1] = {0};
    uint8_t  temp1, temp2;
    uint16_t temp3;

    /****************************************** 初始化IMU ******************************************/

    /* 复位MPU9250 */
    reg[0] = DEV_MPU9250_PWR_MGMT1_REG;
    buf[0] = 0x80;
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /* 等待延时 */
    soc_time_delay_ms(DEV_MPU9250_WAIT_RESTART_TIME_MS);

    /* 读取IMU器件地址 */
    reg[0] = DEV_MPU9250_WHO_AM_I_REG;
    if (s_dev_mpu9250_read_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    if (buf[0] != DEV_MPU9250_IMU_ID)   /* 检验匹配 */
    { 
        return RET_ERROR;
    }

    /* 设置加速度计量程 */
    reg[0] = DEV_MPU9250_ACCEL_CONFIG_REG;
    buf[0] = DEV_MPU9250_IMU_ACC_RANGE_SET;
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /* 设置陀螺仪量程 */
    reg[0] = DEV_MPU9250_GYRO_CONFIG_REG;
    buf[0] = DEV_MPU9250_IMU_GYRO_RANGE_SET;
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /* 设置采样频率(Hz: 4~1000)和低通滤波器的截止频率 */
    temp3 = DEV_MPU9250_IMU_SAMPLE_RATE;        /* <--- 设置采样频率 */ 
    if (temp3 > 1000)
    {
        temp3 = 1000;
    }
    if (temp3 < 4)
    {
        temp3 = 4;
    }
    temp2  = 1000 / (temp3 - 1);   /* 计算1秒采样次数 */
    reg[0] = DEV_MPU9250_SMPLRT_DIV_REG;
    buf[0] = temp2;
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    temp2 = temp3 / 2;  /* 计算LPF截止频率 */
    if (temp2 >= 188)
    {
        temp2 = 1;
    }
    else if (temp2 >= 98)
    {
        temp2 = 2;
    }
    else if (temp2 >= 42)
    {
        temp2 = 3;
    }
    else if (temp2 >= 20)
    {
        temp2 = 4;
    }
    else if (temp2 >= 10)
    {
        temp2 = 5;
    }
    else
    {
        temp2 = 6;
    }
    reg[0] = DEV_MPU9250_CONFIG_REG;
    buf[0] = temp2;
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /* 关闭所有中断 */
    reg[0] = DEV_MPU9250_INT_EN_REG;
    buf[0] = 0x00;
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /* 关闭FIFO */
    reg[0] = DEV_MPU9250_FIFO_EN_REG;
    buf[0] = 0x00;
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /* I2C主模式关闭 */
    reg[0] = DEV_MPU9250_USER_CTRL_REG;
    buf[0] = (0x0 << 5); /* pins ES_DA and ES_SCL are logically driven by pins SDA/SDI and SCL/ SCLK */
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /* INT引脚低电平有效, 开启bypass模式, 可以直接读取磁力计 */
    reg[0] = DEV_MPU9250_INTBP_CFG_REG;
                                        /* i2c_master interface pins(ES_CL and ES_DA) will go into ‘bypass mode’ */
    buf[0] = (0x1 << 7) | (0x1 << 1);/* pins ES_DA and ES_SCL are logically driven by pins SDA/SDI and SCL/ SCLK */
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /* 设置CLKSEL, PLL X轴为参考 */
    reg[0] = DEV_MPU9250_PWR_MGMT1_REG;
                         /* Auto selects the best available clock source – PLL if ready */
    buf[0] = (0x1 << 0);/* pins ES_DA and ES_SCL are logically driven by pins SDA/SDI and SCL/ SCLK */
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /* 加速度与陀螺仪都工作 */
    reg[0] = DEV_MPU9250_PWR_MGMT2_REG;
    buf[0] = (0x0 << 3) | (0x0 << 0); /* accelerometer is on (X, Y, Z); gyro is on (X, Y, Z) */
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    /****************************************** 初始化MAG ******************************************/

    /* 读取MAG器件地址 */
    reg[0] = DEV_MPU9250_MAG_WIA_REG;
    if (s_dev_mpu9250_read_regs(DEV_MPU9250_TARGET_MAG, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    if (buf[0] != DEV_MPU9250_MAG_ID)   /* 检验匹配 */
    { 
        return RET_ERROR;
    }

    /* 设置MAG为单次测量模式 */
    reg[0] = DEV_MPU9250_MAG_CNTL1_REG;
    buf[0] = DEV_MPU9250_MAG_DATA_BIT_SET | DEV_MPU9250_MAG_WORKMODE_SET;
    if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_MAG, reg, buf, 1) != RET_OK)
    {
        return RET_ERROR;
    }

    return RET_OK;
}

/* MPU9250设备初始化 */
RET_STATUS dev_mpu9250_init(void)
{
    /* 分配设备管理结构体 */
    s_dev_rtdrv_mpu9250 = (dev_rtdrv_mpu9250_t) rt_calloc(1, sizeof(dev_rtdrv_mpu9250));

    if (s_dev_rtdrv_mpu9250 == NULL)
    {
        return RET_ERROR;
    }

    /* 找到i2c控制总线 */
    if (s_dev_mpu9250_find_i2c_bus(s_dev_rtdrv_mpu9250) != RET_OK)
    {
        goto fail_handler;
    }

    /* 初始化寄存器 */
    if (s_dev_mpu9250_reg_config() == RET_ERROR)
    {
        goto fail_handler;
    }

    /* 初始化成功 */
    s_dev_rtdrv_mpu9250->init_status = INIT_SUCC;
    return RET_OK;

    /* 初始化失败 */
fail_handler:
    rt_free(s_dev_rtdrv_mpu9250); /* 释放分配到的内存 */

    s_dev_rtdrv_mpu9250->init_status = INIT_FAIL;
    return RET_ERROR;
}

/* 获得MPU9250数据 */
RET_STATUS dev_mpu9250_get_data(uint8_t data_type, void *data)
{
    RET_STATUS     RET = RET_OK;
    uint8_t        reg[1] = {0};
    uint8_t        buf[6] = {0};

    /* 指向全局变量 */
    dt_3axis_s2_t  pacc   = &s_dev_rtdrv_mpu9250->axis_data.acc;
    dt_3axis_s2_t  pgyro  = &s_dev_rtdrv_mpu9250->axis_data.gyro;
    dt_3axis_s2_t  pmag   = &s_dev_rtdrv_mpu9250->axis_data.mag;
    float          *ptemp = &s_dev_rtdrv_mpu9250->temp_data;

    /* 判断buf有效性 */
    if (data == NULL)
    {
        return RET_ERROR;
    }

    /* 判断mpu9250是否初始化完成 */
    if (s_dev_rtdrv_mpu9250->init_status != INIT_SUCC)
    {
        return RET_ERROR;
    }

    /* 判断要读取的数据, 并读取 */
    switch (data_type)
    {
        /* 加速度数据 */
        case DEV_MPU9250_DATA_ACC:
        {
            reg[0] = DEV_MPU9250_ACCEL_XOUT_H_REG;
            memset(buf, 0, sizeof(buf));
            if (s_dev_mpu9250_read_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 6) != RET_OK)
            {
                return RET_ERROR;
            }

            /* 赋值 */
            pacc->x = ((int16_t) buf[0] << 8) | buf[1];
            pacc->y = ((int16_t) buf[2] << 8) | buf[3];
            pacc->z = ((int16_t) buf[4] << 8) | buf[5];

            /* 复制给调用者 */
            memcpy(data, pacc, sizeof(dt_3axis_s2));
        } break;

        /* 陀螺仪数据 */
        case DEV_MPU9250_DATA_GYRO:
        {
            reg[0] = DEV_MPU9250_GYRO_XOUT_H_REG;
            memset(buf, 0, sizeof(buf));
            if (s_dev_mpu9250_read_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 6) != RET_OK)
            {
                return RET_ERROR;
            }

            /* 赋值 */
            pgyro->x = ((int16_t) buf[0] << 8) | buf[1];
            pgyro->y = ((int16_t) buf[2] << 8) | buf[3];
            pgyro->z = ((int16_t) buf[4] << 8) | buf[5];

            /* 复制给调用者 */
            memcpy(data, pgyro, sizeof(dt_3axis_s2));
        } break;

        /* 温度数据 */
        case DEV_MPU9250_DATA_TEMP:
        {
            reg[0] = DEV_MPU9250_TEMP_OUT_H_REG;
            memset(buf, 0, sizeof(buf));
            if (s_dev_mpu9250_read_regs(DEV_MPU9250_TARGET_IMU, reg, buf, 2) != RET_OK)
            {
                return RET_ERROR;
            }

            /* 赋值 */
            *ptemp = ((int16_t) buf[0] << 8) | buf[1];    /* 原始值 */
            *ptemp = ((*ptemp - 0) / 333.87f) + 21;

            /* 复制给调用者 */
            memcpy(data, ptemp, sizeof(float));
        } break;

        /* 地磁数据 */
        case DEV_MPU9250_DATA_MAG:
        {
            reg[0] = DEV_MPU9250_MAG_XOUT_L_REG;
            memset(buf, 0, sizeof(buf));
            if (s_dev_mpu9250_read_regs(DEV_MPU9250_TARGET_MAG, reg, buf, 6) != RET_OK)
            {
                return RET_ERROR;
            }

            /* 赋值 */
            pmag->x = ((int16_t) buf[1] << 8) | buf[0];
            pmag->y = ((int16_t) buf[3] << 8) | buf[2];
            pmag->z = ((int16_t) buf[5] << 8) | buf[4];
            
            /* 复制给调用者 */
            memcpy(data, pmag, sizeof(dt_3axis_s2));

            /* 如果是单次测量, 需要再设置一次单次测量 */
            #if (DEV_MPU9250_MAG_WORKMODE_SET == DEV_MPU9250_MAG_CMD_CTRL1_WM_SM)
            reg[0] = DEV_MPU9250_MAG_CNTL1_REG;
            buf[0] = DEV_MPU9250_MAG_DATA_BIT_SET | DEV_MPU9250_MAG_WORKMODE_SET;
            if (s_dev_mpu9250_write_regs(DEV_MPU9250_TARGET_MAG, reg, buf, 1) != RET_OK)
            {
                return RET_ERROR;
            }
            #endif
        } break;

        default:
        {
            RET = RET_ERROR;
        } break;
    }

    return RET;
}

/* 测试MPU9250罗盘 */
RET_STATUS dev_mpu9250_test_yaw(float *yaw_angle)
{
    dt_3axis_s2_t  pmag = &s_dev_rtdrv_mpu9250->axis_data.mag;
    float         *pyaw = &s_dev_rtdrv_mpu9250->yaw_data;

    /* 获得MPU9250数据 */
    if (dev_mpu9250_get_data(DEV_MPU9250_DATA_MAG, (void *) pmag) != RET_OK)
    {
        return RET_ERROR;
    }
    
    /* 计算水平yaw角 */
    *pyaw      = ((float) atan2(pmag->y, pmag->x) * (180 / DT_PI) + 180);
    *yaw_angle = *pyaw;

    return RET_OK;
}
